//
// Created by zlf on 24-1-4.
//

#ifndef AUTODRIVE_PERCEPTION_MAP_MAP_DECODER_IMPL_H
#define AUTODRIVE_PERCEPTION_MAP_MAP_DECODER_IMPL_H

#include <cmath>
#include <queue>
#include <memory>
#include <vector>
#include <cstdint>
#include <cstring>
#include <algorithm>

#include <iostream>

#include "common/map_info.h"
#include "common/fixed_array.h"
#include "map_decoder/map_decoder.h"

namespace autodrive {
namespace perception {

static constexpr float EPS = std::numeric_limits<float>::epsilon();

class MapDecoderImpl final {
  public:
    MapDecoderImpl();
    MapDecoderImpl(MapDecoderImpl &&) = delete;
    MapDecoderImpl(const MapDecoderImpl &) = delete;
    MapDecoderImpl &operator=(MapDecoderImpl &&) = delete;
    MapDecoderImpl &operator=(const MapDecoderImpl &) = delete;
    ~MapDecoderImpl();

    struct GridPosition {
        /// @brief x
        std::uint32_t x;
        /// @brief y
        std::uint32_t y;
        /// @brief pos
        std::uint32_t pos;
        /// @brief reg
        std::uint32_t reg;
    };

    /// @brief decode mode
    enum class DecodeMode : std::int8_t {
      /// @brief none
      UNKNOWN = 0,
      /// @brief polyline
      POLYLINE = 1,
      /// @brief polygon
      POLYGON = 2,
      /// @brief lane divider
      LANEDIVIDER = 3,
    };

    void Clear() noexcept;

    /// @brief init
    /// @param para map filter parameter
    /// @throw
    void Init(MapDecoder::MapDecoderParameter const &para) noexcept(false);

    /// @brief is init flag
    /// @return init flag
    bool IsInit() noexcept { return is_init_; }

    template <typename T>
    void MapDecode(T const* dpuout_cls, T const* dpuout_reg, std::uint8_t cls,
                   std::uint8_t cls_scale, std::uint8_t reg_scale, Map& map) noexcept(false);

  protected:
    template <typename T>
    void decode_process(T const* dpuout_cls, T const* dpuout_reg, std::uint8_t cls,
                        std::uint8_t cls_scale, std::uint8_t reg_scale,
                        DecodeMode decode_mode, Map& map) noexcept(false);

    void set_result(Map& map, MapPointsElement& cur_points_element, std::uint8_t cls) noexcept(false);

    void post_process(Map& map, std::uint8_t cls) noexcept(false);

    template <typename T>
    void search_line_one_direction_with_joint(T const* dpuout_cls, T const* dpuout_reg, std::uint8_t cls,
                                              std::uint8_t cls_scale, std::uint8_t reg_scale, GridPosition& start_position,
                                              MapPointsElement& cur_map_element, MapElementSet<LaneDividerJoint>& lane_divider_joint) noexcept(false);

    template <typename T>
    void search_line_one_direction(T const* dpuout_cls, T const* dpuout_reg, std::uint8_t cls,
                                   std::uint8_t cls_scale, std::uint8_t reg_scale, GridPosition& start_position,
                                   MapPointsElement& cur_map_element) noexcept(false);

    template <typename T>
    void search_polyline(T const* dpuout_cls, T const* dpuout_reg, std::uint8_t cls,
                         std::uint8_t cls_scale, std::uint8_t reg_scale, GridPosition& start_position,
                         MapPointsElement& cur_map_element) noexcept(false);

    template <typename T>
    void search_polygon(T const* dpuout_cls, T const* dpuout_reg, std::uint8_t cls,
                        std::uint8_t cls_scale, std::uint8_t reg_scale, GridPosition& start_position,
                        MapPointsElement& cur_map_element) noexcept(false);
    template <typename T>
    void search_lane_divider(T const* dpuout_cls, T const* dpuout_reg, std::uint8_t cls,
                             std::uint8_t cls_scale, std::uint8_t reg_scale, GridPosition& start_position,
                             MapPointsElement& cur_map_element, MapElementSet<LaneDividerJoint>& lane_divider_joint) noexcept(false);

    template <typename T>
    void merge_discontinuous_elements(MapElementSet<T>& map_elements, float const merge_threshold) noexcept;

    bool merge_two_line_segments(MapPointsElement& line1, MapPointsElement& line2,
                                 float const  merge_threshold) noexcept;

    void line_smooth(MapPointsElement& line) noexcept;

    inline std::vector<Point3f> get_target_grids(float cur_dir_x, float cur_dir_y,
                                                 float cur_xy_x, float cur_xy_y) noexcept(false);

    template <typename T>
    inline void decode_grid(T const* dpuout_reg, std::uint8_t reg_scale, std::uint32_t x, std::uint32_t y,
                     std::uint32_t pos) noexcept(false);

    inline Point3f distance_decode(float const distance, bool const is_cw) noexcept;

    inline Point3f get_vec(float x0, float y0, float x1, float y1) noexcept;

    inline float get_distance(float const x0, float const y0,
                                     float const x1, float const y1) noexcept {
      return powf(powf(x0 - x1, 2) + powf(y0 - y1, 2), 0.5);
    };

    inline float get_distance_square(float const x0, float const y0, float const x1, float const y1) noexcept {
      return std::pow(x0 - x1, 2) + std::pow(y0 - y1, 2);
    };

    inline float get_theta(float x0, float y0, float x1, float y1) {
      float cos_theta = (x0 * x1 + y0 * y1) / (powf(x0 * x0 + y0 * y0, 0.5) * powf(x1 * x1 + y1 * y1, 0.5) + EPS);
      return acosf(std::min(cos_theta, 1.0f)) * 180.0f / 3.1415926f;
    };

    inline float sigmoid(float const x) noexcept { return (1 / (1 + exp(-x))); };

    inline float inverse_sigmoid(float value) noexcept { return -1.0f * log((1 - value) / (value + EPS)); };

  private:
    /// @brief init flag
    bool is_init_;
    /// @brief  init call once flag
    mutable std::once_flag init_flag_;
    /// @brief map filter paramters
    MapDecoder::MapDecoderParameter parameter_;
    /// @brief net_width
    std::uint32_t net_width_;
    /// @brief net_height
    std::uint32_t net_height_;
    /// @brief detection width lower bound
    std::uint32_t detection_width_lower_bound_;
    /// @brief detection width upper bound
    std::uint32_t detection_width_upper_bound_;
    /// @brief detection height lower bound
    std::uint32_t detection_height_lower_bound_;
    /// @brief detection height upper bound
    std::uint32_t detection_height_upper_bound_;
    /// @brief Decode xy position of each grid
    std::vector<float> dpuout_xy_;
    /// @brief Decode direction of each grid
    std::vector<float> dpuout_vec_;
    /// @brief Decode xy position of each grid
    std::vector<std::uint8_t> visited_;
};

template <typename T>
void MapDecoderImpl::merge_discontinuous_elements(MapElementSet<T>& map_elements, float const merge_threshold) noexcept {
  std::uint32_t filled_size = map_elements.size();
  std::vector<std::uint16_t> keep(filled_size, 1U);

  std::uint16_t i{0}, j{0};
  bool found{false};
  while (i < filled_size) {
    if (!keep[i]) {
      i++;
      continue;
    }

    j = i + 1;
    found = false;
    while (j < filled_size) {
      if (!keep[j]) {
        j++;
        continue;
      }
      found = merge_two_line_segments(map_elements.at(i), map_elements.at(j), merge_threshold);
      if (found) {
        keep[j] = false;
        break;
      } else {
        j++;
      }
    }
    if (!found) {
      i++;
    }
  }

  // move aviailable elements to the front
  std::uint16_t k{0};
  for (i = 0; i < filled_size; ++i) {
      if (!keep[i]) {
        continue;
      }
      if ((i != k)) {
        map_elements.at(k) = map_elements.at(i);
      }
      k++;
  }
  map_elements.resize(k);
}

}  // namespace perception
}  // namespace autodrive
#endif //AUTODRIVE_PERCEPTION_MAP_MAP_DECODER_IMPL_H
